
// F. Chaxel 2010

using namespace std;

#include <iostream>

#include "Robot.h"

Robot* Robot::_instance = NULL;

//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
Robot::Robot()
{

	if (_instance!=NULL)	// I am a Singleton
		throw ;
	
	_instance=this;

	// wheels
	Wheel_Right_Objective=Wheel_Left_Objective=0;
	Wheel_Right_Position=Wheel_Left_Position=0;

	// not running
	in_progress=false;
	kill_robot=false;
	do_emergency_stop=false;
	pthread_spin_init(&lockin_progress, PTHREAD_PROCESS_SHARED); 

	// empty tasks list
	ActionsListPtrHigh=ActionsListPtrLow=0;	
	for (int i=0;i<NB_MAXACTIONS;i++)
		ActionsList[i].type=0;

	Advised_Object=NULL;

	// Init Position
	Position.x=0;
	Position.y=0;
	Position.radian=0;
	TotalMovePosition=0;
}
//////////////////////////////////////////////////////////////////////
Robot::~Robot()
{
	_instance=NULL;
}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
void Robot::Advise(IRobotAdvise *Listener)
{

	if (Advised_Object==NULL)	// only one listener
		Advised_Object=Listener;

}
void Robot::UnAdvise(IRobotAdvise *Listener)
{

	if (Advised_Object==Listener)
		Advised_Object=NULL;	// not really thread safe
}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
void *Robot::RobotAdviseThread( void *ptrAdvise )
{

	//.. used to fire an event in a separated thread
	try
	{
		IRobotAdvise *Advised_Object=(IRobotAdvise *)ptrAdvise;
		Advised_Object->RobotStop();
	}
	catch (...){}

	return NULL;
}
//////////////////////////////////////////////////////////////////////
void Robot::Run()
{	

	for (;;)
	{
		// something to do
		if (in_progress==true)
		{	
			long diffRight, diffLeft;
			unsigned char SpeedRight, SpeedLeft;
	
			// get the actual motor encoder position
			motors.GetPos(&Wheel_Left_Position,&Wheel_Right_Position);

			// compare with the objectives
			diffRight=Wheel_Right_Objective-Wheel_Right_Position;
			diffLeft=Wheel_Left_Objective-Wheel_Left_Position;

			// calculated the needed speed on each wheel
			SpeedRight=CalcWheelSpeed(diffRight,RightMovePlus);
			SpeedLeft=CalcWheelSpeed(diffLeft,LeftMovePlus);

			// set the speed
			motors.SetSpeed (SpeedLeft,SpeedRight);
			(this->*(this->SetPosition))(Wheel_Left_Objective-Wheel_Left_Position, Wheel_Right_Objective-Wheel_Right_Position);

			// if stop, stop running or put the next memorised task
			if ((SpeedRight==128)&&(SpeedLeft==128))
			{
				if (Scheduler(0,0)!=true)	// no new task
				{
					pthread_spin_lock(&lockin_progress); 
						in_progress=false;		// could be made without spin_lock, thread safe
					pthread_spin_unlock(&lockin_progress);

					// fire one event on a separated thread
					if (Advised_Object!=NULL)
					{
						pthread_t idThread;

						pthread_create(&idThread, NULL, &Robot::RobotAdviseThread, Advised_Object);
						sleep(0);
					}
				}
			}

		}
		else
			sleep(0);	// no need to used full system CPU resources

		if (do_emergency_stop==true)
		{

			for (int i=0;i<NB_MAXACTIONS;i++)
				ActionsList[i].type=0;
			ActionsListPtrHigh=ActionsListPtrLow=0;	

			do_emergency_stop=false;
			motors.Stop();
	
			// basicaly here, I have to wait the robot is realy stopped
			// and current position must be stored in
			// objective to avoid bad evalution of previous mouvements
			for (int i=0;i<40;i++)	// not forever
			{
				usleep(100000);	// wait 0,1 s
				motors.GetPos(&Wheel_Left_Objective,&Wheel_Right_Objective);
				
				if ((Wheel_Right_Position==Wheel_Right_Objective)&&(Wheel_Left_Position==Wheel_Left_Objective))
					break;						// OK full stop obtain
				Wheel_Left_Position=Wheel_Left_Objective;
				Wheel_Right_Position=Wheel_Right_Objective;
			}

			pthread_spin_lock(&lockin_progress); 
				in_progress=false;		// could be made without spin_lock, thread safe
			pthread_spin_unlock(&lockin_progress);

		}
	}

}
//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
float Robot::Battery_Voltage()
{
	return (float)motors.GetBattery()/10.0;
}
//////////////////////////////////////////////////////////////////////
bool Robot::Is_Stopped()
{
	bool is_in_progress;

	pthread_spin_lock(&lockin_progress); 
		 is_in_progress=in_progress;	// could be made without spin_lock, thread safe
	pthread_spin_unlock(&lockin_progress);

	return !is_in_progress;
}
/////////////////////////////////////////////////////////////////////
bool Robot::Is_Killed()
{
	return kill_robot;
}
//////////////////////////////////////////////////////////////////////
void Robot::Kill_Robot()
{
	kill_robot=true;
}
//////////////////////////////////////////////////////////////////////
unsigned char Robot::CalcWheelSpeed(long diff, char MoveDirection)
{

	if (MoveDirection==0)
		return 128;

	if ((diff<0)&&(MoveDirection!=1))
		return 128;		// Hoops to fare away !

	if ((diff>0)&&(MoveDirection!=-1))
		return 128;		// Hoops to fare away !
	
	// value betweeen 128 +- (5 to 72)

	if (diff<0) diff=-diff;	
	if (diff>720) diff=720;

	return 128-MoveDirection*(5+diff/10);

}

//////////////////////////////////////////////////////////////////////
// Optimisation of Scheduled actions
//////////////////////////////////////////////////////////////////////
bool Robot::Concat(unsigned char actiontype, long value)
{
	if (ActionsListPtrHigh < 1)
		return false;

	if (ActionsList[ActionsListPtrHigh - 1].type == actiontype)
		if ((ActionsList[ActionsListPtrHigh - 1].value >= 0 && value >= 0) || (ActionsList[ActionsListPtrHigh - 1].value < 0 && value < 0))
		{
			ActionsList[ActionsListPtrHigh - 1].value += value;
			return true;
		}
	return false;
}
//////////////////////////////////////////////////////////////////////
// used to save pushed action into a FIFO stack
// and to pull out the respective action
//////////////////////////////////////////////////////////////////////
bool Robot::Scheduler(unsigned char actiontype, long value)
{

	// action push
	if (actiontype!=0)
	{
		cout << "1" <<endl;
		if (Concat(actiontype, value))
			return true;	// Concataned with previous element
		cout << "2" <<endl;
		if (ActionsList[ActionsListPtrHigh].type!=0)
			return false;	// no more place inside the stack
		cout << "3" <<endl;
		ActionsList[ActionsListPtrHigh].type=actiontype;
		ActionsList[ActionsListPtrHigh].value=value;	
		
		ActionsListPtrHigh+=1;
		ActionsListPtrHigh%=NB_MAXACTIONS;
		return true;
	}
	cout << "4" <<endl;
	if (ActionsList[ActionsListPtrLow].type==0)
		return false;
	cout << "5" <<endl;
	// action pull
	switch (ActionsList[ActionsListPtrLow].type)
	{
		case 1:
			TurnRight(ActionsList[ActionsListPtrLow].value);
			break;
		case 2:
			TurnLeft(ActionsList[ActionsListPtrLow].value);
			break;
		case 3:
			TurnOnPlace(ActionsList[ActionsListPtrLow].value);
			break;
		case 4:
			GoForward(ActionsList[ActionsListPtrLow].value);
			break;
	}

	ActionsList[ActionsListPtrLow].type=0;
	ActionsListPtrLow+=1;
	ActionsListPtrLow%=NB_MAXACTIONS;

	return true;
}
//////////////////////////////////////////////////////////////////////
// Position Absolut
//////////////////////////////////////////////////////////////////////
void Robot::TurnRightPosition(long delta_left,long delta_right){
	double angle = (TotalMovePosition*180/PI*delta_left)/TotalLengthPosition;
	double h = sin(-angle) * WHEEL_DISTANCE / 2;
	Position.y += h;
	Position.x += h/tan((PI-angle)/2);
	Position.radian += angle;
	//cout <<Position.x<< "/" <<Position.y<< "/" <<Position.radian<<endl;
}
void Robot::TurnLeftPosition(long delta_left,long relatif_right){
	double angle = (TotalMovePosition*180/PI*relatif_right)/TotalLengthPosition;
	double h = sin(angle) * WHEEL_DISTANCE / 2;
	Position.y += h;
	Position.x += h/tan((PI-angle)/2);
	Position.radian -= angle;
	//cout <<Position.x<< "/" <<Position.y<< "/" <<Position.radian<<endl;
}
void Robot::GoForwardPosition(long delta_left,long delta_right){
	Position.y += TotalMovePosition*delta_left/TotalLengthPosition*sin(Position.radian);
	Position.x += TotalMovePosition*delta_left/TotalLengthPosition*cos(Position.radian);
	//cout <<Position.x<< "/" <<Position.y<< "/" <<Position.radian<<endl;
}
void Robot::TurnOnPlacePosition(long delta_left,long delta_right){
	double angle = (TotalMovePosition*180/PI*delta_left)/TotalLengthPosition;
	Position.radian += angle;
	//cout <<Position.x<< "/" <<Position.y<< "/" <<Position.radian<<endl;
}

returnFct Robot::SavePosition(unsigned char actiontype, long value, long objectif, long init){
	TotalLengthPosition = objectif - init;
	TotalMovePosition = value;
	switch (actiontype)
	{
		case 1:
			return &Robot::TurnRightPosition;
		case 2:
			return &Robot::TurnLeftPosition;
		case 3:
			return &Robot::GoForwardPosition;
		case 4:
			return &Robot::TurnOnPlacePosition;
	}
}

double Robot::Get_Position(char pos)
{
	switch(pos)
	{
	case 'x':
		return Position.x;
	case 'y':
		return Position.y;
	case 'a' :
		return Position.radian;
	default:
		return 0;
	}
}

//////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////
double Robot::TurnHelper(long degree)
{
	return (WHEEL_DISTANCE*2*PI*degree/CIRCLE_DEGREES)/(WHEEL_DIAMETER*PI/ENCODER_COUNT_SHAFT);
}
bool Robot::TurnRight(long degree)
{
	// if called by the runner thread don't check if robot is running
	if ((Is_Stopped()==false)&&(playThread!=pthread_self()))
		return Scheduler(1, degree);

	Wheel_Left_Objective=(long)(Wheel_Left_Objective - TurnHelper(degree));
	RightMovePlus=0;
	if (degree<0)
		LeftMovePlus=-1;
	else
		LeftMovePlus=+1;

	pthread_spin_lock(&lockin_progress); 
		in_progress=true;
	pthread_spin_unlock(&lockin_progress);

	long l, r;
	motors.GetPos(&l, &r);
	SetPosition = SavePosition(1, degree, Wheel_Left_Objective, l);
	return true;

}
//////////////////////////////////////////////////////////////////////
bool Robot::TurnLeft(long degree)
{
	// if called by the runner thread don't check if robot is running
	if ((Is_Stopped()==false)&&(playThread!=pthread_self()))
		return Scheduler(2,degree);

	Wheel_Right_Objective=(long)(Wheel_Right_Objective - TurnHelper(degree));

	LeftMovePlus=0;
	if (degree<0)
		RightMovePlus=-1;
	else
		RightMovePlus=+1;

	pthread_spin_lock(&lockin_progress); 
		in_progress=true;
	pthread_spin_unlock(&lockin_progress);

	long l, r;
	motors.GetPos(&l, &r);
	SetPosition = SavePosition(2, degree, Wheel_Right_Objective, r);
	return true;

}
//////////////////////////////////////////////////////////////////////
bool Robot::TurnOnPlace(long degree)
{
	// if called by the runner thread don't check if robot is running
	if ((Is_Stopped()==false)&&(playThread!=pthread_self()))
		return Scheduler(3,degree);
	Wheel_Right_Objective=(long)(Wheel_Right_Objective - TurnHelper(degree)/2);
	Wheel_Left_Objective=(long)(Wheel_Left_Objective + TurnHelper(degree)/2);

	if (degree>0)
		LeftMovePlus=-1;
	else
		LeftMovePlus=+1;

	RightMovePlus=-LeftMovePlus;

	pthread_spin_lock(&lockin_progress); 
		in_progress=true;
	pthread_spin_unlock(&lockin_progress);

	long l, r;
	motors.GetPos(&l, &r);
	SetPosition = SavePosition(3, degree, Wheel_Left_Objective, l);
	return true;

}
//////////////////////////////////////////////////////////////////////
bool Robot::GoForward(long millimeter)
{
	// if called by the runner thread don't check if robot is running
	if ((Is_Stopped()==false)&&(playThread!=pthread_self()))
		return Scheduler(4,millimeter);
	Wheel_Right_Objective=Wheel_Right_Objective-millimeter*ENCODER_COUNT_SHAFT/WHEEL_DIAMETER;
	Wheel_Left_Objective=Wheel_Left_Objective-millimeter*ENCODER_COUNT_SHAFT/WHEEL_DIAMETER;
	
	if (millimeter<0)
		LeftMovePlus=-1;
	else
		LeftMovePlus=+1;

	RightMovePlus=LeftMovePlus;

	pthread_spin_lock(&lockin_progress); 
		in_progress=true;
	pthread_spin_unlock(&lockin_progress);
	
	long l, r;
	motors.GetPos(&l, &r);
	SetPosition = SavePosition(4, millimeter, Wheel_Left_Objective, l);

	return true;
}
//////////////////////////////////////////////////////////////////////
void Robot::EmergencyStop()
{
	pthread_spin_lock(&lockin_progress); 
		if (in_progress==true)		
			do_emergency_stop=true;
	pthread_spin_unlock(&lockin_progress);
	kill_robot=true;
}
